
#ifndef TRACKPINJOINT_H_
#define TRACKPINJOINT_H_

#include "PinJoint.h"

namespace p2 {

class TrackPinJoint: public PinJoint
{
DECLARE_RTT(TrackPinJoint, PinJoint);

public:
    inline TrackPinJoint(Anchor const& a0, Anchor const& a1,
                             Vector2<real> a, real length, void* userData);
    virtual ~TrackPinJoint();

	static TrackPinJoint* create(Anchor const& a0, Anchor const& a1,
			Vector2<real> a, real length, void* userData);
	static inline TrackPinJoint* cast(Constraint* rtt);

	inline void setSpring(real k, real b, real restLength);
	inline Vector2<real> const& getLocalA() const;
	inline void setLocalA(Vector2<real> a);
	inline Vector2<real> const& getWorldA() const;
	inline void getWorldLine(Vector2<real>& wxL0, Vector2<real>& wxL1) const;
    /*
     * p2Constraint implementation
     */
    virtual void preStep(real64 dT);
    virtual bool solvePositions(real64 dT);
    virtual bool solveVelocities(real64 dT);
	virtual void computeFutureAnchorPositions(real64 dT);

private:
    /*
     * REF: http://i31www.ira.uka.de/docs/DynamicSimulation.pdf
     * '..joint removes only two translational degrees
     * of freedom.. This joint is defined by
     * a line A + l.a which is fixed to the first body..'
     */
    Vector2<real> _a;
    Vector2<real> _wa, _wla;
    Vector2<real> _rA0th, _rA1th;
    real _restLength;
};

inline TrackPinJoint::TrackPinJoint(Anchor const& a0, Anchor const& a1, Vector2<real> a, real length, void* userData)
:   PinJoint(a0, a1, length, userData),
    _a(a),
    _wa(Vector2<real>::ZERO), _wla(Vector2<real>::ZERO),
    _rA0th(Vector2<real>::ZERO), _rA1th(Vector2<real>::ZERO),
    _restLength(0.f)
{
    _a.normalise();
}

inline TrackPinJoint* TrackPinJoint::cast(Constraint* rtt)
{
	if (rtt->isA(TrackPinJoint::classId())) {
		return (TrackPinJoint*) rtt;
	}
	return 0;
}

inline void TrackPinJoint::setSpring(real k, real b, real restLength)
{
    _k = Math<real>::Max(1.f, k);
    _b = Math<real>::Max(0.f, b);
    _restLength = Math<real>::Clamp(restLength, 0.f, _length);
	_flags |= FLAGS_ENABLE_SPRING;
}

inline Vector2<real> const& TrackPinJoint::getLocalA() const
{
	return _a;
}

inline void TrackPinJoint::setLocalA(Vector2<real> a)
{
	_a = a;
}

inline Vector2<real> const& TrackPinJoint::getWorldA() const
{
	return _wa;
}

inline void TrackPinJoint::getWorldLine(Vector2<real>& wxL0, Vector2<real>& wxL1) const
{
	wxL0 = _xAth + _rA0th;
	wxL1 = _xAth + _rA1th;
}

}

#endif	//TRACKPINJOINT_H_
